#include <ros/ros.h> 
#include <tf/tf.h>
#include <std_msgs/Int32MultiArray.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

// 手臂部分marker半径
#define left_upper_radius 0.034         // 左侧大臂
#define right_upper_radius 0.034        // 右侧大臂
#define left_forearm_radius 0.028       // 左侧小臂
#define right_forearm_radius 0.028      // 右侧小臂
// 高度z_b + z_a * i
#define left_upper_z_b 0.085            // 左侧大臂
#define left_upper_z_a 0.018

#define right_upper_z_b 0.085           // 右侧大臂
#define right_upper_z_a 0.018

#define left_forearm_z_b 0.067          // 左侧小臂
#define left_forearm_z_a 0.018

#define right_forearm_z_b 0.067         // 右侧小臂
#define right_forearm_z_a 0.018

std_msgs::Int32MultiArray read_data;


void get_force_data(const std_msgs::Int32MultiArray::ConstPtr& newdata)
{
    ROS_INFO("Force Data Received!");
    //read_data.data.clear();
    read_data.data = newdata->data;
    
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "tactile_marker0");
    ros::NodeHandle n;
    ros::Rate loop_rate(100); 
    ros::Subscriber sub = n.subscribe("force_data_topic0", 1000, get_force_data);
    ros::Publisher marker_pub = n.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 1);
    uint32_t shape = visualization_msgs::Marker::CYLINDER;

    while (ros::ok())
    {

    

    visualization_msgs::MarkerArray marker_array;
    marker_array.markers.clear();
    
    visualization_msgs::Marker bbox_marker;

    geometry_msgs::Quaternion q;
    double roll,pitch,yaw;

    //marker_pub.publish(marker_array);

    for(int k = 0; k < 10; k++)
    {
        read_data.data.push_back(0.0);
    }
    
    // 左侧大臂
    for(int i = 0; i < 72 ;i++)
    {
        
        bbox_marker.header.frame_id = "/left_upper_arm_link";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        bbox_marker.scale.x = 0.01;
        bbox_marker.scale.y = 0.01;
        bbox_marker.scale.z = 0.0001;
        
        if(i < 6)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = left_upper_radius;            //0.034
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * i;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[0]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 6 && i < 12)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = -left_upper_radius;            
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 6);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[1]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 12 && i < 18)
        {
            bbox_marker.pose.position.x = left_upper_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 12);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[2]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 18 && i < 24)
        {
            bbox_marker.pose.position.x = -left_upper_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 18);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[3]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 24 && i < 30)           // 绕Z轴旋转30度
        {
            bbox_marker.pose.position.x = left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_upper_radius * sin(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 24);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[4]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 30 && i < 36)           // 绕Z轴旋转60度
        {
            bbox_marker.pose.position.x = left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_upper_radius * cos(30.0 / 180.0 * 3.1415926);           
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 30);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[5]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 36 && i < 42)               // 绕Z轴旋转120度
        {
            bbox_marker.pose.position.x = -left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 36);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.094395102;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[6]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 42 && i < 48)           // 绕Z轴旋转150度
        {
            bbox_marker.pose.position.x = -left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 42);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.617993878;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[7]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 48 && i < 54)           // 绕Z轴旋转210度
        {
            bbox_marker.pose.position.x = -left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 48);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 3.665191429;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[8]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 54 && i < 60)           // 绕Z轴旋转240度
        {
            bbox_marker.pose.position.x = -left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_upper_radius * cos(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 54);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 4.188790205;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 60 && i < 66)           // 绕Z轴旋转300度
        {
            bbox_marker.pose.position.x = left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 60);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        if(i >= 66 && i < 72)           // 绕Z轴旋转330度
        {
            bbox_marker.pose.position.x = left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 66);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 0.0;
        }
        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }
    
    // 右侧大臂
    for(int i = 0; i < 72 ;i++)
    {
        
        bbox_marker.header.frame_id = "/right_upper_arm_link";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i + 72;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        bbox_marker.scale.x = 0.01;
        bbox_marker.scale.y = 0.01;
        bbox_marker.scale.z = 0.0001;
        
        if(i < 6)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = right_upper_radius;            //0.034
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * i;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[0]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 6 && i < 12)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = -right_upper_radius;            
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 6);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[1]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 12 && i < 18)
        {
            bbox_marker.pose.position.x = right_upper_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 12);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[2]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 18 && i < 24)
        {
            bbox_marker.pose.position.x = -right_upper_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 18);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[3]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 24 && i < 30)           // 绕Z轴旋转30度
        {
            bbox_marker.pose.position.x = right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_upper_radius * sin(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 24);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[4]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 30 && i < 36)           // 绕Z轴旋转60度
        {
            bbox_marker.pose.position.x = right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_upper_radius * cos(30.0 / 180.0 * 3.1415926);           
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 30);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[5]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 36 && i < 42)               // 绕Z轴旋转120度
        {
            bbox_marker.pose.position.x = -right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 36);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.094395102;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[6]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 42 && i < 48)           // 绕Z轴旋转150度
        {
            bbox_marker.pose.position.x = -right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 42);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.617993878;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[7]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 48 && i < 54)           // 绕Z轴旋转210度
        {
            bbox_marker.pose.position.x = -right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 48);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 3.665191429;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[8]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 54 && i < 60)           // 绕Z轴旋转240度
        {
            bbox_marker.pose.position.x = -right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_upper_radius * cos(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 54);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 4.188790205;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 60 && i < 66)           // 绕Z轴旋转300度
        {
            bbox_marker.pose.position.x = right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 60);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 66 && i < 72)           // 绕Z轴旋转330度
        {
            bbox_marker.pose.position.x = right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 66);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }

    // 左侧小臂
    for(int i = 0; i < 72 ;i++)
    {
        
        bbox_marker.header.frame_id = "/left_forearm_link";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i + 72 + 72;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        bbox_marker.scale.x = 0.01;
        bbox_marker.scale.y = 0.01;
        bbox_marker.scale.z = 0.0001;
        
        if(i < 6)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = left_forearm_radius;            //0.034
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * i;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[0]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 6 && i < 12)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = -left_forearm_radius;            
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 6);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[1]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 12 && i < 18)
        {
            bbox_marker.pose.position.x = left_forearm_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 12);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[2]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 18 && i < 24)
        {
            bbox_marker.pose.position.x = -left_forearm_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 18);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[3]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 24 && i < 30)           // 绕Z轴旋转30度
        {
            bbox_marker.pose.position.x = left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 24);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[4]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 30 && i < 36)           // 绕Z轴旋转60度
        {
            bbox_marker.pose.position.x = left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);           
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 30);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[5]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 36 && i < 42)               // 绕Z轴旋转120度
        {
            bbox_marker.pose.position.x = -left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 36);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.094395102;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[6]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 42 && i < 48)           // 绕Z轴旋转150度
        {
            bbox_marker.pose.position.x = -left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 42);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.617993878;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[7]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 48 && i < 54)           // 绕Z轴旋转210度
        {
            bbox_marker.pose.position.x = -left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 48);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 3.665191429;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[8]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 54 && i < 60)           // 绕Z轴旋转240度
        {
            bbox_marker.pose.position.x = -left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 54);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 4.188790205;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 60 && i < 66)           // 绕Z轴旋转300度
        {
            bbox_marker.pose.position.x = left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 60);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 66 && i < 72)           // 绕Z轴旋转330度
        {
            bbox_marker.pose.position.x = left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 66);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }
  
    // 右侧小臂
    for(int i = 0; i < 72 ;i++)
    {
        
        bbox_marker.header.frame_id = "/right_forearm_link";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i + 72 + 72 + 72;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        bbox_marker.scale.x = 0.01;
        bbox_marker.scale.y = 0.01;
        bbox_marker.scale.z = 0.0001;
        
        if(i < 6)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = right_forearm_radius;            //0.034
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * i;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[0]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 6 && i < 12)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = -right_forearm_radius;            
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 6);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[1]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 12 && i < 18)
        {
            bbox_marker.pose.position.x = right_forearm_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 12);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[2]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 18 && i < 24)
        {
            bbox_marker.pose.position.x = -right_forearm_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 18);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[3]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 24 && i < 30)           // 绕Z轴旋转30度
        {
            bbox_marker.pose.position.x = right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 24);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[4]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 30 && i < 36)           // 绕Z轴旋转60度
        {
            bbox_marker.pose.position.x = right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);           
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 30);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[5]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 36 && i < 42)               // 绕Z轴旋转120度
        {
            bbox_marker.pose.position.x = -right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 36);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.094395102;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[6]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 42 && i < 48)           // 绕Z轴旋转150度
        {
            bbox_marker.pose.position.x = -right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 42);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.617993878;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[7]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 48 && i < 54)           // 绕Z轴旋转210度
        {
            bbox_marker.pose.position.x = -right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 48);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 3.665191429;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[8]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 54 && i < 60)           // 绕Z轴旋转240度
        {
            bbox_marker.pose.position.x = -right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 54);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 4.188790205;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 60 && i < 66)           // 绕Z轴旋转300度
        {
            bbox_marker.pose.position.x = right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 60);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        if(i >= 66 && i < 72)           // 绕Z轴旋转330度
        {
            bbox_marker.pose.position.x = right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 66);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            bbox_marker.color.r = read_data.data[9]/3000.0;
            bbox_marker.color.g = 0.5;
            bbox_marker.color.b = 0.5;
            bbox_marker.color.a = 1.0;
        }
        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }

    while (marker_pub.getNumSubscribers() < 1)
    {
        if (!ros::ok())
        {
            return 0;
        }
        ROS_WARN_ONCE("Please create a subscriber to the marker");
        sleep(1);
    }
        marker_pub.publish(marker_array);

    //ros::spin();
    
    ros::spinOnce(); 
    
    loop_rate.sleep(); 
    //ros::spin();
    }
}
